import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from foxglove_msgs.msg import CompressedImage
from std_msgs.msg import Int32MultiArray 
from builtin_interfaces.msg import Time
import cv2
import numpy as np
from cv_bridge import CvBridge
#from .submodules.my_opencv import process_image

class ImageRepublisher(Node):
    def __init__(self):
        super().__init__('image_republisher')
        self.subscription = self.create_subscription(
            Image,
            '/image',
            self.listener_callback_image,
            10)
        self.publisher = self.create_publisher(CompressedImage, '/image_xy', 10)
        self.subscription = self.create_subscription(
            Int32MultiArray,
            '/xy_coordinates',
            self.listener_callback_xy,
            10)
        self.bridge = CvBridge()  # 用于将ROS图像消息转换为OpenCV格式
        self.x = 0
        self.y = 0

    def listener_callback_xy(self,msg):
        self.x = msg.data[0]
        self.y = msg.data[1]

    def listener_callback_image(self, msg):
        np_arr = np.frombuffer(msg.data, np.uint8)
        cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
        cv2.circle(cv_image, (self.x, self.y), 5, (0, 255, 0), 3)
        _, buffer = cv2.imencode('.jpeg', cv_image)
        
        # 创建 CompressedImage 消息
        compressed_msg = CompressedImage()
        compressed_msg.timestamp = Time()
        compressed_msg.timestamp.sec = msg.header.stamp.sec
        compressed_msg.timestamp.nanosec = msg.header.stamp.nanosec
        compressed_msg.frame_id = msg.header.frame_id
        compressed_msg.format = 'jpeg'
        #compressed_msg.data = msg.data
        compressed_msg.data = buffer.tobytes()
        # 发布压缩后的图像消息
        self.publisher.publish(compressed_msg)
        
        

def main(args=None):
    rclpy.init(args=args)
    image_republisher_xy = ImageRepublisher()
    rclpy.spin(image_republisher_xy)
    rclpy.shutdown()

if __name__ == '__main__':
    main()